#include <iostream>
#include "minAreaRect.h"

using namespace cv;
using namespace std;

//#define MINAREARECT_DEBUG

// 直线拟合 Ax+By+C=0；
// 5个rst 分别为 A，B，C meanX，meanY，
void LineFitLeastSquares(float* data_x, float* data_y, int data_n, float rst[5])
{
    float Sxx = 0.0;
    float Sx = 0.0;
    float Sxy = 0.0;
    float Sy = 0.0;
    float Syy = 0.0;

    for (int i = 0; i < data_n; i++)
    {
        Sxx += data_x[i] * data_x[i];
        Sx += data_x[i];
        Sxy += data_x[i] * data_y[i];
        Sy += data_y[i];
        Syy += data_y[i] * data_y[i];
    }

    // 计算斜率a和截距b  
    float a, b, t1,t2;
    t1 = data_n * Sxx - Sx * Sx;
    t2 = data_n * Syy - Sy * Sy;
    if (t1 > t2) // y = ax + b
    {
        a = (data_n * Sxy - Sx * Sy) / t1;
        b = (Sxx * Sy - Sx * Sxy) / t1;
        rst[0] = a;
        rst[1] = -1;
        rst[2] = b;
    }
    else // x = ay + b
    {
        a = (data_n * Sxy - Sx * Sy) / t2;
        b = (Syy * Sx - Sy * Sxy) / t2;
        rst[0] = -1;
        rst[1] = a;
        rst[2] = b;
    }

   
    rst[3] = Sx / data_n;
    rst[4] = Sy / data_n;
}

void LineFitLeastSquares(const vector<Point>& points, vector<float>& rst)
{
    int n = points.size();
    vector<float> X(n), Y(n);
    rst.resize(5);
    for (int i = 0; i < n; ++i)
    {
        X[i] = points[i].x;
        Y[i] = points[i].y;
    }
    LineFitLeastSquares(X.data(), Y.data(), n, rst.data());
}

void LineFitLeastSquares(const vector<Point2f>& points, vector<float>& rst)
{
    int n = points.size();
    vector<float> X(n), Y(n);
    rst.resize(5);
    for (int i = 0; i < n; ++i)
    {
        X[i] = points[i].x;
        Y[i] = points[i].y;
    }
    LineFitLeastSquares(X.data(), Y.data(), n, rst.data());
}

//旋转边界点，旋转角theta，旋转中心center
void rotateContour(const vector<Point>& contour, vector<Point>& rotatedContour, double theta, Point center)
{
    rotatedContour.resize(contour.size());
    for (int i = 0; i < contour.size(); ++i)
    {
        float x = contour[i].x - center.x;
        float y = contour[i].y - center.y;
        rotatedContour[i].x = x * cos(-theta) - y * sin(-theta);
        rotatedContour[i].y = x * sin(-theta) + y * cos(-theta);

        rotatedContour[i].x += center.x;
        rotatedContour[i].y += center.y;
    }
}

//找中线，要求边界已经水平放置
void getMidline(const vector<Point>& contour0, vector<Point>& midline)
{
    auto contour = contour0;
    std::sort(contour.begin(), contour.end(), [](Point p1, Point p2) {return (p1.x < p2.x) || (p1.x == p2.x && p1.y < p2.y); });
    int cur_x = contour[0].x;
    float max_y, min_y = contour[0].y;
    for (int i = 0; i < contour.size(); ++i)
    {
        if (contour[i].x > cur_x)
        {
            max_y = contour[i - 1].y;

            if (max_y != min_y)
            {
                midline.emplace_back(Point{ cur_x, int(min_y + max_y) / 2 });                
            }
            cur_x = contour[i].x;
            min_y = contour[i].y;
        }
    }
}

RotatedRect minAreaRect2(vector<Point>& contour)
{
    vector<float>rst;
    double theta;
    Point center;
    auto rrect = minAreaRect(contour);
    if (rrect.size.height < rrect.size.width)
    {
        theta = rrect.angle / 180.0 * CV_PI;
    }
    else
    {
        theta = (rrect.angle + 90) / 180.0 * CV_PI;
    }
    center = rrect.center;

    // LineFitLeastSquares(contour, rst);// 最小二乘找对称线
    // theta = atan2(rst[0],-rst[1]);
    // center = { (int)rst[3],(int)rst[4] };

    vector<Point> rotatedContour;
    rotateContour(contour, rotatedContour, theta, center);
   
    vector<Point> midline;
    getMidline(rotatedContour, midline);
    
    LineFitLeastSquares(midline ,rst);
    double theta2 = atan2(rst[0], -rst[1]);
    theta += theta2;

    rotateContour(contour, rotatedContour, theta, center);

    
    Rect rect = boundingRect(rotatedContour);
    Point p0, p1;
    Size2f sz;
    float angle;

    if (theta > 0)
    {
        angle = theta * 180.0 / CV_PI - 90.0;
        sz.width = rect.height;
        sz.height = rect.width;
    }
    else
    {
        angle = theta * 180.0 / CV_PI;
        sz.width = rect.width;
        sz.height = rect.height;
    }
    p0 = (rect.tl() + rect.br()) / 2;
    p0 -= center;
    p1.x = p0.x * cos(theta) - p0.y * sin(theta);
    p1.y = p0.x * sin(theta) + p0.y * cos(theta);
    p1 += center;

    RotatedRect rotrect;
    rotrect.center = p1;
    rotrect.size = sz;
    rotrect.angle = angle;

#ifdef  MINAREARECT_DEBUG
    int w = 2 * MAX(rect.br().x, rotrect.center.x);
    int h = 2 * MAX(rect.br().y, rotrect.center.y);
    Mat img11 = Mat::zeros(h, w, CV_8UC3);
    vector<vector<Point>>tmp = { contour };
    vector<vector<Point>>tmp2 = { rotatedContour };

    /*for (auto p : midline)
    {
        circle(img11, p, 2, Scalar(255, 0, 255), CV_FILLED);
    }
    for (auto p : contour)
    {
        circle(img11, p, 2, Scalar(255, 255, 255), CV_FILLED);
    }*/
    line(img11, Point(p1.x-rect.height*cos(theta),p1.y-rect.height*sin(theta)), Point(p1.x + rect.height * cos(theta), p1.y + rect.height * sin(theta)), Scalar(255, 255, 0));
    drawContours(img11, tmp, 0, Scalar(0, 255, 0));
    drawContours(img11, tmp2, 0, Scalar(0,0,255));
    rectangle(img11, rect, Scalar(0, 128, 255));
    ellipse(img11, rotrect, Scalar(30, 255, 122));
    imshow("minarearect2", img11);
    //waitKey(0);
#endif
    return rotrect;

}

void Rotate(const Mat& srcImage, Mat& destImage, double angle)
{
    Point2f center(srcImage.cols / 2, srcImage.rows / 2);//中心
    Mat M = getRotationMatrix2D(center, angle, 1);//计算旋转的仿射变换矩阵 
    warpAffine(srcImage, destImage, M, srcImage.size());//仿射变换  
    //circle(destImage, center, 2, Scalar(255, 0, 0));
}

// int main()
// {
//     Mat img = Mat(300, 300, CV_8UC1,Scalar::all(0));
//     Mat img2 = Mat(300, 300, CV_8UC3, Scalar::all(0));

//     Mat leaf = imread("leaf.jpg",CV_8UC1); 
//     leaf.copyTo(img(Rect(100,100,leaf.cols,leaf.rows)));
//     threshold(img, img, 128, 255, CV_THRESH_BINARY);
//     Rotate(img, img, -60);
//     vector<vector<Point>> contours2;
//     vector<Vec4i> hierarchy2;
//     findContours(img, contours2, hierarchy2, RETR_TREE, CHAIN_APPROX_NONE);
//     drawContours(img2, contours2, 0, Scalar(0, 255, 0));
   
//     imshow("img", img);
//     imshow("img2", img2);
//     minAreaRect2(contours2[0]);
//     waitKey(0);
// }